\section{Collision and Contact Detection}

\htmlmenu{2}

A key component of the GraspIt! engine is the collision and contact
detection. When collisions are enabled, anytime you interact with a
GraspIt! world, either by moving an object or using the joint draggers
on a robot, the engine will prevent any movement that brings two
objects in collision and stop it at the moment of contact. Points of
contact are then marked with contact indicators.

Important note: although GraspIt! (through the Inventor scene graph)
can display all of these geometry types, the collision detection
system only works with triangles. When a body geometry is loaded,
GraspIt! will use the Inventor scene graph to facet it into triangles,
which are then used to build the body model for the collision
detection system.

In GraspIt! there is a very important difference between contact and
collision. We define collision as two bodies that are
interpenetrating, no matter by how much. In general, most algorithms
in GraspIt! consider collision to be an \textbf{illegal} state and
will attempt to find collision-free states. Contact is defined as two
bodies that are closer together than a given threshold, but are
\textbf{not} interpenetrating. In general, this threshold is set to be
$0.1mm$. If you would like to change this threshold, you will have to
go inside the code: it is the \texttt{THRESHOLD} static member of the
\texttt{Contact} class.

You can also enable or disable collisions, for either the entire
simulation world, one particular body, or a pair of bodies. This is
all done using the \texttt{Toggle Collision} switch in the main
GraspIt!  interface. Depending on what is selected when you press the
button, the following will happen:
\begin{itemize}
\item if nothing is selected, collisions are toggled for the entire
  simulation world
\item if a single body is selected, collisions are toggled for that
  particular body (it can / can not collide with any other body in the
  world)
\item if a pair of bodies is selected, collisions are toggled for that
  particular pair (they can / can not collide with each other;
  collisions with all the other bodies in the world are unaffected).
\end{itemize}

Most GraspIt! functionality that involves moving bodies around
(including user interaction with joint draggers) works as follows:
\begin{itemize}
\item move the bodies freely as long as there is no collision;
\item when collision is detected, attempt to interpolate between the
  collision state and the last known collision-free state until the
  bodies are no longer colliding, but are separated by less than the
  contact threshold;
\item find all points where two bodies are separated by less than the
  contact threshold and mark them as contacts.
\end{itemize}

\subsection{Contacts}

In GraspIt! a contact is defined as any point where two bodies are
separated by less than the contact threshold, but \textbf{not}
interpenetrating. The collision detection engine will find these
points for you, and also do some pruning, as explained below. A
GraspIt! contact, defined in the \texttt{Contact} class, encapsulates
the following information:
\begin{itemize}
\item the location of the contact on each of the two bodies (the
  points on the two bodies that are separated by less than the contact
  threshold)
\item the contact normal (defined as the normalized vector between the
  two points mentioned above)
\item the Contact Wrench Space: the space of forces and torques that
  can be applied at the contact. This is a crucial concept, which is
  at the base of most grasp quality computations and dynamic
  simulations. In the simplest case, that of Coulomb friction, this
  space is a 3D cone. See the \link{Publications}{sec:publications}
  section of this document for more details.
\item a visual marker showing the location of the contact in the
  GraspIt! GUI. In the case of rigid body contacts, this is actually a
  rendering of the friction cone, aligned with the contact normal.
\end{itemize}

Note that everywhere in GraspIt! we refer to contacts as
\textbf{points}. The reason is that geometry in GraspIt! never
deforms, so we can never explicitly compute \textbf{areas} of
contact. There are two important aspects though. First, two rigid
bodies might be locally similar, so that more than one point is within
the contact threshold. If that happens, the collision detection just
returns many point contacts in a small area. All these point contacts
are then pruned, keeping only those contacts that are on the contour
of the area (the boundary of the convex hull of contact
locations). According to the theory on contacts that we rely on, this
will have no net effect on any grasp quality computations, but will
reduce computation time by reducing the number of contacts).

Second, GraspIt! can simulate some of the effects of soft bodies in
contact without explicitly computing the deformation. This is done by
using a different version of the Contact Wrench Space. See the
\link{Soft Contacts}{sec:softFingers} chapter of this manual for
details.

\subsection{Software implementation}

For many algorithms, the collision engine is the computational
bottleneck. It is very important to have an efficient engine, but we
also require this engine to be very robust and work well on triangle
meshes (as opposed to analytical primitives). The GPL version of
GraspIt! comes with its own collision detection implementation, using
a number of common bounding box hierarchy methods. However, fast
collision detection is a research area in itself, and we are sure
there are many ways to improve our implementation. Any bug reports,
patches or optimizations for the collision detection engine are highly
appreciated.

From a software perspective, we have built the collision and contact
detection libraries to be as modular as possible. This allows the
complete replacement of our current collision detection with the
library of your choice. If you are interested in doing this, check out
the \texttt{CollisionInterface} class and its subclasses. If you know
of a good collision detection library that is fast and robust, works
with triangle meshes, and has a GPL-compatible license, we would love
to hear about it.

\subsection{Under development: Multi-threading}

We have also implemented a crude multi-threaded support for the
collision detection mechanism. This is still very much work in
progress, both from a design and implementation standpoint. The overall
concept is as follows: if you have multiple threads in your GraspIt!
code, each thread can add its own bodies to the simulation world. The
rule we have implemented is that bodies from different threads never
collide with each other. The only exception is that all bodies collide
with bodies from the main thread.

The reason for this implementation is to allow you to test multiple
scenarios in parallel, without worrying about collision detection. As
an example, suppose you have a hand and a glass sitting on a table,
and you want to evaluate many grasps quickly. In this case, you would
populate the world with the table and the glass in the main
thread. Then you would fire up many threads, each loading its own copy
of the hand. Each thread can then tests its own grasps independently,
without needing to synchronize with the other threads or worrying
about colliding with the hands from other threads. Of course, if your
code is single-threaded, you can just ignore all of this and pretend
it does not exist.

The general steps for GraspIt! multi-threading are:
\begin{itemize}
\item in the main thread, load all the objects that will be shared
  between threads
\item fire up all of your computation threads. In each thread, inform
  the collision detection mechanism that it is now servicing a new
  thread (see the CollisionInterface class for what method to call for
  this)
\item in each thread, load all the objects or robots that are specific
  to that thread
\item in each thread, run any computation like you normally would.
\end{itemize}

For examples, see the \texttt{EGPlanner} class which has support for
running in its own thread.

\subsection{Under development: Object cloning}

We also have a mechanism for "cloning" objects in GraspIt! so you can
easily create multiple copies of a body or robot without having to
load it from a file multiple times and without wasting memory. As far
as any GraspIt! algorithm is concerned, a clone is a totally
independent body with its own position in the world, contacts,
etc. However, under the hood, a clone will share all the scene graph
geometry and collision detection bounding box hierarchy with the
original. See the \texttt{Body::cloneFrom(...)} and
\texttt{Robot::cloneFrom(...)}  methods for details. This mechanism
works well, with one exception: we never implemented a nice cleanup
phase that handles the case where the original is deleted and the
clone still lingers in the world. This situation is almost guaranteed
to cause a crash.

In practice, cloning and multi-threading often go together. If you
have lots of computation that you might parallelize, you can create
clones of your moving objects (usually the robots and hands) and pass
them to different threads, where each thread will do its own work. See
again the \texttt{EGPlanner} class for details - it can run in its own
thread using a cloned hand for computations.

